// This file is part of OpenMeca, an easy software to do mechanical simulation.
//
// Author(s)    :  - Damien ANDRE  <openmeca@gmail.com>
//
// Copyright (C) 2012 Damien ANDRE
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.


// This source file was inspired of the "libGeometrical" from 
// the GranOO workbench : http://www.granoo.org



#ifndef _OpenMeca_Geom_MATRIX_hpp_
#define _OpenMeca_Geom_MATRIX_hpp_

#include <cmath>
#include <iostream>
#include <cassert>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

#include "OpenMeca/Geom/Vector.hpp"
#include "OpenMeca/Geom/Frame.hpp"


namespace OpenMeca
{
  namespace Geom
  {
    
  
    //**********************************
    // Proxy Class to get operator [][]
    //**********************************
    template<SpaceDim N>
    class proxy
    {
    public:
      proxy(Coordinate<N> *m): m_(m){};
      double& operator[](unsigned int c) {return  (*m_)[c];};
    private:
      Coordinate<N> *m_;
    };

    template<SpaceDim N>
    class proxyConst
    {
    public:
      proxyConst(const Coordinate<N> *m): m_(m){};
      //double& operator[](unsigned int c) {return  m_[c];};
      const double& operator[](unsigned int c) const {return  (*m_)[c];};
    private:
      Coordinate<N> const *m_;
    };
    //***************************************
    //***************************************


    template<SpaceDim N> class Matrix;
    
    template<>
    class Matrix<_3D>
    {
    public:
      Matrix(boost::function<const Frame<_3D>& ()> = &Frame<_3D>::GetGlobal);
      Matrix(double, double, double, double, double, double, double, double, double, boost::function<const Frame<_3D>& ()> = &Frame<_3D>::GetGlobal);
      Matrix(const Matrix<_3D>&);
      Matrix(const Matrix<_3D>&, boost::function<const Frame<_3D>& ()>);
      Matrix& operator=(const Matrix<_3D>&);
      static std::string GetStrKey(){return std::string("Matrix" + SpaceDimUtil<_3D>::GetStrKey());}
      
      
     
      proxy<_3D> operator[](unsigned int l){ return proxy<_3D>(c_[l]); };
      const proxyConst<_3D> operator[](unsigned int l) const {return proxyConst<_3D>(c_[l]);};
      
      double GetValue(unsigned int i, unsigned int j) const;
      void SetValue(unsigned int i, unsigned int j, double val);

      
      void Clear();
      const Frame<_3D>& GetFrame() const;
      double GetDeterminant();
      Matrix<_3D> GetTranspose();
      Matrix<_3D> GetInverse();
      
      friend Vector<_3D> operator* (const Matrix<_3D> &m, const Vector<_3D> &v);
      friend Matrix<_3D> operator* (const double &d, const Matrix<_3D> &m);
      friend Matrix<_3D> operator* (const Matrix<_3D> &m, const double &d);
      friend Matrix<_3D> operator* (const Matrix<_3D> &m1, const Matrix<_3D> &m2);
      friend Matrix<_3D> operator/ (const Matrix<_3D> &m, const double &d);

      
      const Coordinate<_3D>& GetLine0() const {return c0_;}
      const Coordinate<_3D>& GetLine1() const {return c1_;}
      const Coordinate<_3D>& GetLine2() const {return c2_;}

      boost::function<const Frame<_3D>& ()>& GetFrameFunctionAccess() {return c0_.GetFrameFunctionAccess();}
      const boost::function<const Frame<_3D>& ()>& GetFrameFunctionAccess() const {return c0_.GetFrameFunctionAccess();}
  
    private:
      //double m_[N*N]; //-------->[line][colums] || [x] = [3*l + c]
      Coordinate<_3D> c0_, c1_, c2_;
      Coordinate<_3D> *c_[3];

      //BOOST SERIALIZATION       
      friend class boost::serialization::access;
      template<class Archive>
      void serialize(Archive & ar, const unsigned int ) 
      {
	//a simple way to serialize without saving frame tree
	//be carefull...
	ar  &boost::serialization::make_nvp("line0", c0_);
	ar  &boost::serialization::make_nvp("line1", c1_);
	ar  &boost::serialization::make_nvp("line2", c2_);
      }
     

    };
    
    template<SpaceDim M> Vector<M> operator* (const Matrix<M> &m, const Vector<M> &v);
    template<SpaceDim M> Matrix<M> operator* (const double &d, const Matrix<M> &m);
    template<SpaceDim M> Matrix<M> operator* (const Matrix<M> &m, const double &d);
    template<SpaceDim M> Matrix<M> operator* (const Matrix<M> &m1, const Matrix<M> &m2);
    template<SpaceDim M> Matrix<M> operator/ (const Matrix<M> &m, const double &d);
    template<SpaceDim N> std::ostream& operator<< (std::ostream& o, const Matrix<_3D>& M);

    //
    //EXTERNALS OPERATORS
    //
    inline 
    std::ostream& 
    operator<< (std::ostream& o, const Matrix<_3D>& M)
    { 
      return o << M[0][0] << '\t' << M[0][1] << '\t' << M[0][2] << '\n'
	       << M[1][0] << '\t' << M[1][1] << '\t' << M[1][2] << '\n'
	       << M[2][0] << '\t' << M[2][1] << '\t' << M[2][2] << '\n';
    }
    
     inline Vector<_3D> 
    operator*(const Matrix<_3D> &m, const Vector<_3D> &v)
    {
      return Vector<_3D>(m.c0_.c_[0]*v[0] + m.c0_.c_[1]*v.coord_.c_[1] + m.c0_.c_[2]*v.coord_.c_[2],
			 m.c1_.c_[0]*v.coord_.c_[0] + m.c1_.c_[1]*v.coord_.c_[1] + m.c1_.c_[2]*v.coord_.c_[2],
			 m.c2_.c_[0]*v.coord_.c_[0] + m.c2_.c_[1]*v.coord_.c_[1] + m.c2_.c_[2]*v.coord_.c_[2],v.GetFrameFunctionAccess());

    }
    
    inline Matrix<_3D> 
    operator*(const double &d, const Matrix<_3D> &m)
    {
      return Matrix<_3D>(m.c0_.c_[0]*d, m.c0_.c_[1]*d, m.c0_.c_[2]*d,
			 m.c1_.c_[0]*d, m.c1_.c_[1]*d, m.c1_.c_[2]*d,
			 m.c2_.c_[0]*d, m.c2_.c_[1]*d, m.c2_.c_[2]*d, 
			 m.GetFrameFunctionAccess());
    }
    
    inline Matrix<_3D> 
    operator*(const Matrix<_3D> &m, const double &d)
    {
      return d*m;
    }
    
    inline Matrix<_3D> 
    operator* (const Matrix<_3D> &m1, const Matrix<_3D> &m2)
    {
      //assert(m1.GetFrame() == m2.GetFrame());
      return Matrix<_3D>(m1.c0_.c_[0]*m2.c0_.c_[0] + m1.c0_.c_[1]*m2.c1_.c_[0] + m1.c0_.c_[2]*m2.c2_.c_[0],
			 m1.c0_.c_[0]*m2.c0_.c_[1] + m1.c0_.c_[1]*m2.c1_.c_[1] + m1.c0_.c_[2]*m2.c2_.c_[1],
			 m1.c0_.c_[0]*m2.c0_.c_[2] + m1.c0_.c_[1]*m2.c1_.c_[2] + m1.c0_.c_[2]*m2.c2_.c_[2],

			 m1.c1_.c_[0]*m2.c0_.c_[0] + m1.c1_.c_[1]*m2.c1_.c_[0] + m1.c1_.c_[2]*m2.c2_.c_[0],
			 m1.c1_.c_[0]*m2.c0_.c_[1] + m1.c1_.c_[1]*m2.c1_.c_[1] + m1.c1_.c_[2]*m2.c2_.c_[1],
			 m1.c1_.c_[0]*m2.c0_.c_[2] + m1.c1_.c_[1]*m2.c1_.c_[2] + m1.c1_.c_[2]*m2.c2_.c_[2],

			 m1.c2_.c_[0]*m2.c0_.c_[0] + m1.c2_.c_[1]*m2.c1_.c_[0] + m1.c2_.c_[2]*m2.c2_.c_[0],
			 m1.c2_.c_[0]*m2.c0_.c_[1] + m1.c2_.c_[1]*m2.c1_.c_[1] + m1.c2_.c_[2]*m2.c2_.c_[1],
			 m1.c2_.c_[0]*m2.c0_.c_[2] + m1.c2_.c_[1]*m2.c1_.c_[2] + m1.c2_.c_[2]*m2.c2_.c_[2],
			 m1.GetFrameFunctionAccess());
    }


    inline Matrix<_3D> 
    operator/(const Matrix<_3D> &m, const double &d)
    {
      return Matrix<_3D>(m.c0_.c_[0]/d, m.c0_.c_[1]/d, m.c0_.c_[2]/d,
			 m.c1_.c_[0]/d, m.c1_.c_[1]/d, m.c1_.c_[2]/d,
			 m.c2_.c_[0]/d, m.c2_.c_[1]/d, m.c2_.c_[2]/d,
			 m.GetFrameFunctionAccess());
    }
    

    //
    //CONSTRUCTOR
    //
    inline 
    Matrix<_3D>::Matrix(double d0, double d1, double d2, double d3, double d4, double d5, double d6, double d7, double d8, boost::function<const Frame<_3D>& ()> f)
      : c0_(d0,d1,d2,f), c1_(d3,d4,d5,f), c2_(d6,d7,d8,f) 
    {
      c_[0] = &c0_;
      c_[1] = &c1_;
      c_[2] = &c2_;
    }

  
    
    inline 
    Matrix<_3D>::Matrix(boost::function<const Frame<_3D>& ()> f)
    :c0_(0,0,0,f), c1_(0,0,0,f), c2_(0,0,0,f)
    {
      c_[0] = &c0_;
      c_[1] = &c1_;
      c_[2] = &c2_;
    }
    
    inline 
    Matrix<_3D>::Matrix(const Matrix<_3D>& M)
      :c0_(M.c0_), c1_(M.c1_), c2_(M.c2_)
    {
      c_[0] = &c0_;
      c_[1] = &c1_;
      c_[2] = &c2_;
    }
    
    inline Matrix<_3D> & 
    Matrix<_3D>::operator=(const Matrix<_3D>& M)
    {
      c0_ = M.c0_;
      c1_ = M.c1_;
      c2_ = M.c2_;
      return *this;
    }
    

    inline double 
    Matrix<_3D>::GetValue(unsigned int i, unsigned int j) const
    {
      return (*this)[i][j];
    }

    
    inline void 
    Matrix<_3D>::SetValue(unsigned int i, unsigned int j, double val)
    {
      (*this)[i][j] = val;
    } 
    //
    //UTIL METHOD
    //
    
    inline double 
    Matrix<_3D>::GetDeterminant()
    {
      return c0_.c_[0]*(c1_.c_[1]*c2_.c_[2]-c1_.c_[2]*c2_.c_[1]) - c0_.c_[1]*(c1_.c_[0]*c2_.c_[2]-c1_.c_[2]*c2_.c_[0]) + c0_.c_[2]*(c1_.c_[0]*c2_.c_[1]-c1_.c_[1]*c2_.c_[0]);
    }
    
    inline Matrix<_3D> 
    Matrix<_3D>::GetTranspose()
    {
      return Matrix<_3D>(c0_.c_[0],c1_.c_[0],c2_.c_[0],c0_.c_[1],c1_.c_[1],c2_.c_[1],c0_.c_[2],c1_.c_[2],c2_.c_[2],GetFrameFunctionAccess());
    }
    
    inline Matrix<_3D> 
    Matrix<_3D>::GetInverse()
    {
      const double det = GetDeterminant();
      return Matrix<_3D>(c1_.c_[1]*c2_.c_[2] - c2_.c_[1]*c1_.c_[2],
			 c2_.c_[0]*c1_.c_[2] - c1_.c_[0]*c2_.c_[2],
			 c1_.c_[0]*c2_.c_[1] - c2_.c_[0]*c1_.c_[1],
			 c2_.c_[1]*c0_.c_[2] - c0_.c_[1]*c2_.c_[2],
			 c0_.c_[0]*c2_.c_[2] - c2_.c_[0]*c0_.c_[2],
			 c2_.c_[0]*c0_.c_[1] - c0_.c_[0]*c2_.c_[1],
			 c0_.c_[1]*c1_.c_[2] - c1_.c_[1]*c0_.c_[2],
			 c1_.c_[0]*c0_.c_[2] - c0_.c_[0]*c1_.c_[2],
			 c0_.c_[0]*c1_.c_[1] - c1_.c_[0]*c0_.c_[1],
			 GetFrameFunctionAccess()
			 ).GetTranspose()/det;
    }  

    inline void 
    Matrix<_3D>::Clear()
    {
      c0_.Clear();
      c1_.Clear();
      c2_.Clear();
    }
    
    inline const Frame<_3D> &
    Matrix<_3D>::GetFrame() const
    {
      return c0_.GetFrame();
    }
  

  }
}	



#endif

